#include <Dynamics/b2World.h>
#include <Dynamics/b2Body.h>
#include <Dynamics/b2Fixture.h>
#include <Common/b2Math.h>

#include <Collision/Shapes/b2PolygonShape.h>

#include <main/Globals.h>
#include "CollisionManager.h"
#include "DebugDrawBox2d.h"
#include <collisions/Entity.h>
#include <main/MainConstants.h>
#include <math\Utils.h>
#include "math\Vector3.h"
#include <Common/b2Math.h>

//#define DEBUG_DRAW_COLLISIONS

#define CENTER_X (WINDOW_WIDTH / 2 )
#define CENTER_Y (WINDOW_HEIGHT /2 )

CollisionManager::CollisionManager() {
	Globals::gGameLoop->updateMe(this);
	
	b2Vec2 gravity(0.0f, 0.0f); 
	world = new b2World(gravity);	
	world->SetContactListener(&contactListener);
	createGroundBody();

#ifdef DEBUG_DRAW_COLLISIONS
	debugDraw = new Box2dDrawDebug();
	debugDraw->SetFlags(b2Draw::e_shapeBit | b2Draw::e_aabbBit);
	
	world->SetDebugDraw(debugDraw);
#endif
}

CollisionManager::~CollisionManager() {
	delete world;
#ifdef DEBUG_DRAW_COLLISIONS
	delete debugDraw;
#endif

}


void CollisionManager::createGroundBody() { 
	top = createCollider(BOX_SIZE, 10, true, true);
	bottom = createCollider(BOX_SIZE, 10, true, true);
	left = createCollider(10, BOX_SIZE, true, true);
	right = createCollider(10, BOX_SIZE, true, true);
	left->SetTransform(b2Vec2((CENTER_X - (BOX_SIZE / 2) - 5) * PIXEL_TO_METER , CENTER_Y * PIXEL_TO_METER), 0);
	right->SetTransform(b2Vec2((CENTER_X + (BOX_SIZE / 2) + 5) * PIXEL_TO_METER , CENTER_Y * PIXEL_TO_METER), 0);
	top->SetTransform(b2Vec2(CENTER_X * PIXEL_TO_METER, (CENTER_Y - (BOX_SIZE / 2) - 5) * PIXEL_TO_METER), 0);
	bottom->SetTransform(b2Vec2(CENTER_X * PIXEL_TO_METER, (CENTER_Y + (BOX_SIZE / 2) + 5) * PIXEL_TO_METER), 0);

	left->GetFixtureList()->SetRestitution(1);
	right->GetFixtureList()->SetRestitution(1);
	top->GetFixtureList()->SetRestitution(1);
	bottom->GetFixtureList()->SetRestitution(1);
}

void CollisionManager::destroy(b2Body* body) {
	//Entity* entity = (Entity*)body->GetUserData();
	//entity->isMarkedForDelete = true;
	queueForDelete.push_back(body);
}

b2Body* CollisionManager::createCollider(float w, float h, bool phisicsOn, bool staticBody) {
	b2BodyDef bodyDef; 
	bodyDef.type = staticBody ? b2_staticBody : b2_dynamicBody;
	bodyDef.gravityScale = 0.0f; 
	bodyDef.angularDamping = 0;
	bodyDef.linearDamping = 0;
	bodyDef.allowSleep = false; 

	b2Body* body = world->CreateBody(&bodyDef); 
	body->SetUserData(nullptr);

	// Create the shape
	b2PolygonShape shape; 
	shape.SetAsBox((w * PIXEL_TO_METER) / 2, (h * PIXEL_TO_METER) / 2);

	b2FixtureDef fixtureDef; 
	fixtureDef.friction = 0;
	fixtureDef.density = 1;
	fixtureDef.restitution = 1;
	fixtureDef.shape = &shape; 
	if(!phisicsOn) {
		fixtureDef.isSensor = true;
	}

	body->CreateFixture(&fixtureDef);
	return body;
}

void CollisionManager::update() {
	static const float timeStep = 1.0f / 60.0f; 
	static const int velocityIterations = 6; 
	static const int positionIterations = 2; 

	world->Step(timeStep, velocityIterations, positionIterations); 
	for(int i = 0; i < queueForDelete.size(); ++i) {
		world->DestroyBody(queueForDelete[i]);
	}
	queueForDelete.clear();
}

void CollisionManager::drawDebug() {
	world->DrawDebugData();
}
